#!/usr/bin/env python
import rospy

from webots_ros.msg import Int32Stamped

from webots_ros.srv import set_float

robot_namespace = 'sdnyc_webots'
TIMESTEP = 32

lpos = 0
rpos = 0

ppos = 0
tpos = 0


## ORV (off-road vehicle) view
#
# FL  |-------|  FR
#     |  CAM  |
#     |       |
#     |       |
# BL  |-------|  BR
#

# front left | front right 
fl_wheel_client = '{}/front_left_wheel/set_position'.format(robot_namespace)
fr_wheel_client = '{}/front_right_wheel/set_position'.format(robot_namespace)

# back left | back right 
bl_wheel_client = '{}/back_left_wheel/set_position'.format(robot_namespace)
br_wheel_client = '{}/back_right_wheel/set_position'.format(robot_namespace)


# ptz_pan_client = 'ptz_cam/pan_joint/set_position'
# ptz_tilt_client = 'ptz_cam/tilt_joint/set_position'

rospy.wait_for_service(fl_wheel_client)
fl_set_pos = rospy.ServiceProxy(fl_wheel_client, set_float, persistent=True)
rospy.wait_for_service(fr_wheel_client)
fr_set_pos = rospy.ServiceProxy(fr_wheel_client, set_float, persistent=True)
rospy.wait_for_service(bl_wheel_client)
bl_set_pos = rospy.ServiceProxy(bl_wheel_client, set_float, persistent=True)
rospy.wait_for_service(br_wheel_client)
br_set_pos = rospy.ServiceProxy(br_wheel_client, set_float, persistent=True)

# rospy.wait_for_service(ptz_pan_client)
# pan_set_pos = rospy.ServiceProxy(ptz_pan_client, set_float, persistent=True)
# rospy.wait_for_service(ptz_tilt_client)
# tilt_set_pos = rospy.ServiceProxy(ptz_tilt_client, set_float, persistent=True)


def __keyboard_callback(msg):
    global lpos, rpos
    global ppos, tpos

    key = msg.data
    send = False

    if key == 314: # left-arrow
        lpos += -0.2
        rpos +=  0.2
        send = True
    if key == 315: # up-arrow
        lpos +=  0.5
        rpos +=  0.5
        send = True
    if key == 316: # right-arrow
        lpos +=  0.2
        rpos += -0.2
        send = True
    if key == 317: # down-arrow
        lpos += -0.5
        rpos += -0.5
        send = True
    
    if key == 65: # a
        ppos +=  0.1
        send = True
    if key == 87: # w
        tpos += -0.1
        send = True
    if key == 68: # d
        ppos += -0.1
        send = True
    if key == 83: # s
        tpos += 0.1
        send = True
    
    if send:
        fl_set_pos(lpos)
        bl_set_pos(lpos)

        fr_set_pos(rpos)
        br_set_pos(rpos)

        # pan_set_pos(ppos)
        # tilt_set_pos(tpos)

        rospy.loginfo('Left position: {} | Right position: {}.'.format(lpos, rpos))

if __name__ == "__main__":
    rospy.init_node('webots_keyboard_position_controller', anonymous=False)
    rospy.loginfo('Keyboard controller node initialized. ')

    # subscribe keyboard topic
    __keyboard_topic = '%s/keyboard' % robot_namespace
    rospy.Subscriber(__keyboard_topic+'/key', Int32Stamped, callback=__keyboard_callback)

    rospy.loginfo('Keyboard controller started. Use arrow keys (or AWSD) to navigate the ORV. ESC to exit.')
    
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

